#include "ROB_Sensorshield.h"
#include "ROB_i2cBusMaster.h"
#include "ROB_Telemetry.h"


void CSensorShield::Begin(void) {
  _lastLight = 0;
  _lastSound = 0;
  _lastPingEnable = 0;
  _lastIrBarEnable = 0;
  createI2cCmd();
  I2cBusmaster.send(I2C_SENSORSHIELD_ADR, 1, I2C_SENSORSHIELD_ACK_LEN, _i2cCmd);
}


uint8_t CSensorShield::update(void) {


  // check if we received a sensor update

  if(I2cBusmaster.ackReceived(I2C_SENSORSHIELD_ADR, I2C_SENSORSHIELD_ACK_LEN)) {
    uint8_t * rcvBuf = I2cBusmaster.getAck();
    tmy.btnStatus = rcvBuf[0];
    tmy.pingDistance = rcvBuf[1];
    tmy.ir1Distance = rcvBuf[2];
    tmy.ir2Distance = rcvBuf[3];
    tmy.ir3Distance = rcvBuf[4];
    tmy.ir4Distance = rcvBuf[5];
    tmy.ir5Distance = rcvBuf[6];
    tmy.irMaxDistance = rcvBuf[7];
  }

  // send command
  if(_lastLight != tmy.lightSystem 
    || _lastSound != tmy.soundPlay  
    || _lastPingEnable != (tmy.actorEnablers & (1 << TMY_ENA_PING))
    || _lastIrBarEnable != (tmy.actorEnablers & (1 << TMY_ENA_IRBAR))
    ) {

    createI2cCmd();
    if (I2cBusmaster.send(I2C_SENSORSHIELD_ADR, 1, I2C_SENSORSHIELD_ACK_LEN, _i2cCmd) == I2C_ERROR_NONE) {
      _lastLight = tmy.lightSystem;
      _lastSound = tmy.soundPlay;
      _lastPingEnable = tmy.actorEnablers & (1 << TMY_ENA_PING);
      _lastIrBarEnable = tmy.actorEnablers & (1 << TMY_ENA_IRBAR);
    }
    else {
      return I2C_ERROR_LOCKED;
    }

  }

}




uint8_t CSensorShield::requestSensors(void) {
  // only request status
  if(I2cBusmaster.send(I2C_SENSORSHIELD_ADR, 0, I2C_SENSORSHIELD_ACK_LEN, NULL) != I2C_ERROR_NONE) 
    return I2C_ERROR_LOCKED;
  return I2C_ERROR_NONE;

}





void CSensorShield::createI2cCmd() {

  _i2cCmd[0] = (tmy.soundPlay & 0x07);
  _i2cCmd[0] <<= 2;
  _i2cCmd[0] |= (tmy.lightSystem & 0x03);
  _i2cCmd[0] <<= 1;
  _i2cCmd[0] |= ((tmy.actorEnablers >> TMY_ENA_IRBAR) & 0x01);
  _i2cCmd[0] <<= 1;
  _i2cCmd[0] |= ((tmy.actorEnablers >> TMY_ENA_PING) & 0x01);

}


CSensorShield Sensorshield;

